//
// Created by Wirano on 2020/11/9.
//

//导航相关文件 但是貌似没什么用
#include "Ano_Navigate.h"
#include "Ano_Math.h"
#include "Ano_Imu.h"

/// 用于规范进度在一个正确的范围之类
/// \param x
/// \return
int32_t dlon_180(int32_t x) //10^-7
{
    return (x>1800000000?(x-3600000000):(x<-1800000000?(x+3600000000):x));

}// 最后这个函数貌似没有用到

void dlon_dlat_dx_dy(int32_t lon,int32_t lat,int32_t lon_ref,int32_t lat_ref,int32_t *dx,int32_t *dy ) //10^-7  ->  cm
{
    int32_t dlon_t;
    float lon_cos;

    dlon_t = dlon_180(lon - lon_ref);
    lon_cos = my_cos( ABS( (int16_t)(lat/10000000) ) *ANGLE_TO_RADIAN );

    *dx = 1.117f *dlon_t *lon_cos;
    *dy = 1.117f *(lat - lat_ref); //10^-7 *1000 *100 = 0.01f锛?.01f *111.7	

}

